#include "inc/dev_imu.h"
#include "driver/i2c.h"

#define  delay_ms(x)  vTaskDelay(x / portTICK_PERIOD_MS)

void imu_WriteReg(uint8_t reg_addr, uint8_t reg_val)
{
    i2c_cmd_handle_t cmd_obj = i2c_cmd_link_create();

    i2c_master_start(cmd_obj);					//起始信号
    i2c_master_write_byte(cmd_obj, ((MPU6050_IIC_ADDR << 1) | I2C_MASTER_WRITE), 0x1);	//从机地址及读写位
    i2c_master_write_byte(cmd_obj, reg_addr, 0x1);
    i2c_master_write_byte(cmd_obj, reg_val, 0x1);
    i2c_master_stop(cmd_obj);

    i2c_master_cmd_begin(common_i2c_num, cmd_obj, 100 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd_obj);
}

uint8_t imu_ReadReg(uint8_t reg_addr)
{
    uint8_t data;
    i2c_cmd_handle_t cmd_obj = i2c_cmd_link_create();
    i2c_master_start(cmd_obj);					//起始信号
    i2c_master_write_byte(cmd_obj, ((MPU6050_IIC_ADDR << 1) | I2C_MASTER_WRITE), 0x1);	//从机地址及读写位
    i2c_master_write_byte(cmd_obj, reg_addr, 0x1);

    i2c_master_start(cmd_obj);
    i2c_master_write_byte(cmd_obj, ((MPU6050_IIC_ADDR << 1) | I2C_MASTER_READ), 0x1);
    i2c_master_read_byte(cmd_obj,&data, I2C_MASTER_NACK);
    i2c_master_stop(cmd_obj);					//终止信号

    i2c_master_cmd_begin(common_i2c_num, cmd_obj, 100 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd_obj);
    return data;
}

uint8_t imu_ReadRegUnit(uint8_t reg_addr, uint8_t num, uint8_t *data)
{

    i2c_cmd_handle_t cmd_obj = i2c_cmd_link_create();
    i2c_master_start(cmd_obj);					//起始信号
    i2c_master_write_byte(cmd_obj, ((MPU6050_IIC_ADDR << 1) | I2C_MASTER_WRITE), 0x1);	//从机地址及读写位
    i2c_master_write_byte(cmd_obj, reg_addr, 0x1);

    i2c_master_start(cmd_obj);
    i2c_master_write_byte(cmd_obj, ((MPU6050_IIC_ADDR << 1) | I2C_MASTER_READ), 0x1);

    i2c_master_read(cmd_obj,data,num,I2C_MASTER_LAST_NACK);

    i2c_master_stop(cmd_obj);					//终止信号

    i2c_master_cmd_begin(common_i2c_num, cmd_obj, 100 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd_obj);
    return ESP_OK;
}


void imu_reset(void)
{
    imu_WriteReg(MPU_PWR_MGMT1_REG,0x80);
	delay_ms(100);
    imu_WriteReg(MPU_PWR_MGMT1_REG,0x00);
}


void imu_set_gyro_fsr(uint8_t fsr)
{
    imu_WriteReg(MPU_GYRO_CFG_REG, fsr << 3);
}

void imu_set_accel_fsr(uint8_t fsr)
{
    imu_WriteReg(MPU_ACCEL_CFG_REG, fsr << 3);
}


void imu_set_lpf(uint16_t lpf)
{
    uint8_t dat;

    if (lpf >= 188)     dat = 1;
    else if (lpf >= 98) dat = 2;
    else if (lpf >= 42) dat = 3;
    else if (lpf >= 20) dat = 4;
    else if (lpf >= 10) dat = 5;
    else  dat = 6;

    imu_WriteReg(MPU_CFG_REG, dat);
}

void imu_set_rate(uint16_t rate)
{
    uint8_t ret;
    uint8_t dat;

    if (rate > 1000) rate = 1000;
    if (rate < 4)rate = 4;

    dat = 1000 / rate - 1;
    imu_WriteReg(MPU_SAMPLE_RATE_REG, dat);
    imu_set_lpf(rate >> 1);
}


uint8_t imu_get_accelerometer(int16_t *ax, int16_t *ay, int16_t *az)
{
    uint8_t dat[6];
    uint8_t ret;

    ret =  imu_ReadRegUnit( MPU_ACCEL_XOUTH_REG,6,dat);

    *ax = ((uint16_t)dat[0] << 8) | dat[1];
    *ay = ((uint16_t)dat[2] << 8) | dat[3];
    *az = ((uint16_t)dat[4] << 8) | dat[5];
    return ret;
}

uint8_t imu_get_gyroscope(int16_t *gx, int16_t *gy, int16_t *gz)
{
    uint8_t dat[6];
    uint8_t ret;
    ret =  imu_ReadRegUnit( MPU_GYRO_XOUTH_REG,6,dat);
    *gx = ((uint16_t)dat[0] << 8) | dat[1];
    *gy = ((uint16_t)dat[2] << 8) | dat[3];
    *gz = ((uint16_t)dat[4] << 8) | dat[5];
    return ret;
}

uint8_t imu_get_temperature(int16_t *temp)
{
    uint8_t dat[2];
    uint8_t ret;
    int16_t raw = 0;
    ret =  imu_ReadRegUnit( MPU_TEMP_OUTH_REG,2,dat);

    raw = ((uint16_t)dat[0] << 8) | dat[1];
    *temp = (int16_t)((36.53f + ((float)raw / 340)) * 100);
    return ret;
}



void mpu6050_init(void)
{
    uint8_t imu_id = 0;
    imu_reset();
    imu_set_gyro_fsr(3);
    imu_set_accel_fsr(0);
    imu_set_rate(50);
    imu_WriteReg(MPU_INT_EN_REG, 0x00);
    imu_WriteReg(MPU_USER_CTRL_REG, 0x00);
    imu_WriteReg(MPU_FIFO_EN_REG, 0x00);
    imu_WriteReg(MPU_INTBP_CFG_REG, 0x80);

    imu_id = imu_ReadReg(MPU_DEVICE_ID_REG);

    if (imu_id != MPU6050_IIC_ADDR){
        printf("mpu6050 init failed!\r\n");
        return;
    }

    imu_WriteReg(MPU_PWR_MGMT1_REG, 0x01);
    imu_WriteReg(MPU_PWR_MGMT2_REG, 0x00);
    imu_set_rate(50);
    printf("mpu6050 init succeed!\r\n");

}
